/**
  ******************************************************************************
  * @file           : ble_fifo.c
  * @author			: duanzhe
  * @brief          : BLE数据传输缓冲区（FIFO）源文件
  *                   包含FIFO队列初始化、数据进队、出队处理
  ******************************************************************************
  * @attention
  *
  * Copyright (c) 2023 乐普医电.
  *
  ******************************************************************************
  */
#include "ble_fifo.h"

#define MOD(X,Y) (((X) >= (Y)) ? ((X)-(Y)) : (X))

fifo_t BleRxFifoQueue;
fifo_t BleTxFifoQueue;

/**
  * @brief   初始化Fifo环形队列
  * @param  q: 指向Fifo数据结构体
  * @param  fifoBuffer: 指向用作Fifo的数组首地址
  * @param  fifomaxsize:  Fifo队列的最大容量（按字节）
  * @retval 0: 初始化成功  -1:初始化失败
  */
int FifoQueue_Init(fifo_t *q, uint8_t* fifoBuffer, uint32_t fifomaxsize)
{
	if (fifoBuffer == NULL)
	{
		return -1;
	}
	q->fifoBuffer = fifoBuffer;
	q->ptr_out = 0;
	q->ptr_in = 0;
	q->bytecount = 0;
	q->datacount = 0;
	q->fifomaxsize = fifomaxsize;
	return 0;
}

/**
  * @brief  向Fifo队列中添加数据
  * @note   可以添加一个或者多个数据，可以将不定长的数据添加到Fifo队列中
  * @param  q: 指向Fifo数据结构体
  * @param  X; 指向要被添加到队列中的数据首地址
  * @param  datasizes:  数据本身所占用的字节长度
  * @param  numdatas: 要添加的数据的个数
  * @retval  返回添加的最后一个数据的首地址，如果添加失败就返回NULL
  */
uint8_t* FifoQueue_DataIn(fifo_t *q, uint8_t* x, uint16_t datasizes, uint32_t numdatas)
{
	uint8_t* ptr = NULL;                      				/* 添加后最后一个数据的首地址 */
	uint16_t curDataSize = 0;              					/* 当前队列中in指针指向的数据自身的大小 */
	uint32_t curFifoPosition;                   			/* 当前写入的数据应该在队列中所在的位置 */
	uint32_t i;                               				/* 循环计数 */
	uint32_t NbBytesToCopy = 0, NbCopiedBytes = 0 ; 		/* 在队列中要拷贝和已经拷贝的数据个数 */


	/* 获取in指针所指向的那个数据的长度，长度数据存放在每个数据的开头第一个和第二个字节位置上 */
	if (q->bytecount)
	{
		curDataSize = q->fifoBuffer[q->ptr_in] + ((q->fifoBuffer[MOD((q->ptr_in+1), q->fifomaxsize)])<<8) + 2;
	}
	/* 检查Fifo中是否有足够空位用于数据存放，数据的存放格式为每个数据的长度 + 数据本体 */
	/* 数据占用字节数 =（数据本身所占字节 + 长度数据的2个字节）* 要存放的数据个数 */
	if (datasizes && ((q->bytecount + ((datasizes + 2 )*numdatas)) <= q->fifomaxsize))
    {
		/* loop to add all elements  */
		for (i=0; i < numdatas; i++)
		{
			/* 将in指针移动到Fifo中下一个可用的空位上，并先在这个空位上将当前要写入的数据的长度先写进去 */
			q->ptr_in = MOD ((q->ptr_in + curDataSize),q->fifomaxsize);
			curFifoPosition = q->ptr_in;
			/* curFifoPosition始终指向当前要写的数据应该写入的位置，这个值在递增之后需要修正，超出边界后继续从头部开始 */
			q->fifoBuffer[curFifoPosition++]= datasizes & 0xFF;
			curFifoPosition = MOD(curFifoPosition, q->fifomaxsize);
			q->fifoBuffer[curFifoPosition++]= (datasizes & 0xFF00) >> 8 ;
			curFifoPosition = MOD(curFifoPosition, q->fifomaxsize);
			q->bytecount += 2;

			/* 计算从当前位置开始到Fifo边界可以写入整个数据，还是需要将数据进行拆分 */
			NbBytesToCopy = MIN((q->fifomaxsize - curFifoPosition), datasizes);
			/* 将可以一次拷贝的数据先进行拷贝  */
			memcpy(&q->fifoBuffer[curFifoPosition],&x[i*datasizes],NbBytesToCopy);
			/* 修改队列中的字节数t */
			q->bytecount += NbBytesToCopy;
			/* 将数据写入的位置置0，用于后面未写完的数据继续从Fifo头部开始写 */
			curFifoPosition = 0;
			/* 设置已经写入的数据个数 */
			NbCopiedBytes = NbBytesToCopy;
			/* 还剩余的未写入的数据个数 */
			NbBytesToCopy = datasizes - NbBytesToCopy;
			/* 设置当前写入的数据所占字节数大小，用于下一次循环开始定位数据写入的地址 */
			curDataSize = datasizes + 2 ;

			/* 如果还有数据未写完 */
			if (NbBytesToCopy)
			{
				memcpy(&q->fifoBuffer[curFifoPosition],&x[(i*datasizes) + NbCopiedBytes], NbBytesToCopy);
				q->bytecount += NbBytesToCopy;
			}
			/* 队列中的数据个数递增 */
			q->datacount++;
		}
		ptr = q->fifoBuffer + (MOD((q->ptr_in + 2 ),q->fifomaxsize));
    }
	else
	{
		return NULL;
	}
  return ptr;
}

/**
  * @brief  从Fifo队列中出队一个数据.
  * @param  q: 指向Fifo数据结构体
  * @param  datasizes:  出队数据的大小（不包含长度数据多出的2个字节）
  * @retval 出队的数据的地址
  */
uint8_t* FifoQueue_DataOut(fifo_t *q, uint16_t* datasizes)
{
	uint8_t* ptr= NULL;
	uint16_t eltSize = 0;
	if (q->bytecount > 0)
	{
		/* 读取Out指针指向的数据大小 */
		eltSize = q->fifoBuffer[q->ptr_out] + ((q->fifoBuffer[MOD((q->ptr_out+1), q->fifomaxsize)])<<8);
		/* 从Out指针位置开始往后移动2个位置，找到数据头 */
		ptr = q->fifoBuffer + (MOD((q->ptr_out + 2), q->fifomaxsize));
		/* 队列中的字节数减少 */
		q->bytecount -= (eltSize + 2) ;
		/* 移动Out指针到下一个可用的数据上 */
		if (q->bytecount > 0)
		{
			q->ptr_out = MOD((q->ptr_out+ eltSize + 2 ), q->fifomaxsize);
		}
		/* 队列中的数据个数递减 */
		--q->datacount;
	}
	if (datasizes != NULL)
	{
		*datasizes = eltSize;
	}
	return ptr;
}



